#!/usr/bin/env python

import rospy

from sensor_msgs.msg import Image

from webots_ros.srv import set_int

robot_namespace = 'sdnyc_webots'
TIMESTEP = 32


if __name__ == '__main__':
    rospy.init_node('webots_ros_spawner', anonymous=False)

    # wait for webots ros controller to start
    try:
        rospy.loginfo('Waiting for webots ros controller...')
        rospy.wait_for_service('%s/robot/get_time' % robot_namespace, timeout=40)
        rospy.loginfo('Webots ros controller found.')
    except:
        rospy.logerr('Webots ros controller may not work properly. Please check.')    


    ## left camera
    __left_camera_topic = 'sdnyc_webots/left_camera/enable'
    # enable camera
    rospy.wait_for_service(__left_camera_topic)
    rospy.loginfo('Trying to enable left camera...')    
    enable_left_camera = rospy.ServiceProxy(__left_camera_topic, set_int)
    try:
        enable_left_camera(TIMESTEP)
        rospy.loginfo('left camera enabled.')
    except rospy.ServiceException as exc:
        rospy.logerr("{} did not process request: {}".format(__left_camera_topic, str(exc)))
    
    ## right camera
    __right_camera_topic = 'sdnyc_webots/right_camera/enable'
    # enable camera
    rospy.wait_for_service(__right_camera_topic)
    rospy.loginfo('Trying to enable right camera...')    
    enable_right_camera = rospy.ServiceProxy(__right_camera_topic, set_int)
    try:
        enable_right_camera(TIMESTEP)
        rospy.loginfo('right camera enabled.')
    except rospy.ServiceException as exc:
        rospy.logerr("{} did not process request: {}".format(__right_camera_topic, str(exc)))

    # ## gps
    # __gps_topic = '%s/gps/enable' % robot_namespace
    # # enable gps
    # rospy.wait_for_service(__gps_topic)
    # rospy.loginfo('Trying to enable GPS...')
    # enable_gps = rospy.ServiceProxy(__gps_topic, set_int)
    # try:
    #     enable_gps(TIMESTEP)
    #     rospy.loginfo('GPS enabled.')
    # except rospy.ServiceException as exc:
    #     rospy.logerr("{} did not process request: {}".format(__gps_topic, str(exc)))


    ## Accelerometer
    __accel_topic = 'sdnyc_webots/accelerometer/enable'
    # enable accelerometer
    rospy.wait_for_service(__accel_topic)
    enable_accel = rospy.ServiceProxy(__accel_topic, set_int)
    try:
        enable_accel(TIMESTEP)
        rospy.loginfo('Accelerometer enabled.')
    except rospy.ServiceException as exc:
        rospy.logerr("{} did not process request: {}".format(__accel_topic, str(exc)))

    ## Gyro
    __gyro_topic = 'sdnyc_webots/gyro/enable'
    # enable gyro
    rospy.wait_for_service(__gyro_topic)
    enable_gyro = rospy.ServiceProxy(__gyro_topic, set_int)
    try:
        enable_gyro(TIMESTEP)
        rospy.loginfo('Gyro enabled.')
    except rospy.ServiceException as exc:
        rospy.logerr("{} did not process request: {}".format(__gyro_topic, str(exc)))
  

    ## LiDAR
    __lidar_topic = 'sdnyc_webots/Velodyne_Puck/enable'
    # enable LiDAR
    rospy.wait_for_service(__lidar_topic)
    enable_lidar = rospy.ServiceProxy(__lidar_topic, set_int)
    try:
        enable_lidar(TIMESTEP)
        rospy.loginfo('LiDAR enabled.')
    except rospy.ServiceException as exc:
        rospy.logerr("{} did not process request: {}".format(__lidar_topic, str(exc)))


    ## keyboard
    __keyboard_topic = 'sdnyc_webots/keyboard/enable'
    # enable IMU
    rospy.wait_for_service(__keyboard_topic)
    enable_keyboard = rospy.ServiceProxy(__keyboard_topic, set_int)
    try:
        enable_keyboard(4)
        rospy.loginfo('Keyboard enabled.')
    except rospy.ServiceException as exc:
        rospy.logerr("{} did not process request: {}".format(__keyboard_topic, str(exc)))


    ## wheel sensors
    __wheel_sensor_topic = '/sdnyc_webots/{}_wheel_sensor/enable'
    __wheel_name_list = ['front_left', 'front_right', 'back_left', 'back_right']
    # enable wheel sensors

    for n in __wheel_name_list:
        topic_name = __wheel_sensor_topic.format(n)
        rospy.wait_for_service(topic_name)
        rospy.loginfo('Trying to enable {}_wheel_sensor...'.format(n))    
        enable_wheel_sensor = rospy.ServiceProxy(topic_name, set_int)
        try:
            enable_wheel_sensor(TIMESTEP)
            rospy.loginfo('Wheel sensor enabled.')
        except rospy.ServiceException as exc:
            rospy.logerr("{} did not process request: {}".format(topic_name, str(exc)))
    
